/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <map>
#include <string>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "slam/io/proto_stream.h"
#include "slam/io/proto_stream_deserializer.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/2d/probability_grid.h"
#include "slam/mapping/2d/submap_2d.h"
#include "slam/mapping/3d/submap_3d.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/ros_map.h"

namespace slam_ros {

class MapPublisher : public rclcpp::Node {
 public:
  explicit MapPublisher(const rclcpp::NodeOptions& options)
      : Node("slam_pbstream_map_publisher", options) {
    this->declare_parameter<std::string>("pbstream_filename", "");
    this->declare_parameter<std::string>("map_topic", "map");
    this->declare_parameter<std::string>("map_frame_id", "map");
    this->declare_parameter<double>("resolution", 0.05);

    pbstream_filename_ = this->get_parameter("pbstream_filename").as_string();
    map_topic_ = this->get_parameter("map_topic").as_string();
    map_frame_id_ = this->get_parameter("map_frame_id").as_string();
    resolution_ = this->get_parameter("resolution").as_double();

    if (pbstream_filename_.empty()) {
      RCLCPP_ERROR(this->get_logger(), "-pbstream_filename is missing.");
      rclcpp::shutdown();
      return;
    }

    publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
        map_topic_, rclcpp::QoS(1).transient_local());

    PublishMap();
  }

 private:
  void PublishMap() {
    try {
      auto msg = LoadOccupancyGridMsg(pbstream_filename_, resolution_);
      
      RCLCPP_INFO(this->get_logger(), 
                  "Publishing occupancy grid topic %s (frame_id: %s, resolution: %f)",
                  map_topic_.c_str(), map_frame_id_.c_str(), resolution_);
                  
      publisher_->publish(msg);
    } catch (const std::exception& e) {
      RCLCPP_ERROR(this->get_logger(), "Map publishing failed: %s", e.what());
      rclcpp::shutdown();
    }
  }

  nav_msgs::msg::OccupancyGrid LoadOccupancyGridMsg(
      const std::string& pbstream_filename, const double resolution) {
    try {
      ::slam::io::ProtoStreamReader reader(pbstream_filename);
      ::slam::io::ProtoStreamDeserializer deserializer(&reader);

      RCLCPP_INFO(this->get_logger(), "Loading submap slices from serialized data.");
      std::map<::slam::mapping::SubmapId, ::slam::io::SubmapSlice> submap_slices;
      ::slam::mapping::ValueConversionTables conversion_tables;
      ::slam::io::DeserializeAndFillSubmapSlices(
          &deserializer, &submap_slices, &conversion_tables);

      if (!reader.eof()) {
        throw std::runtime_error("Failed to read pbstream file completely");
      }

      RCLCPP_INFO(this->get_logger(), "Generating combined map image from submap slices.");
      const auto painted_slices =
          ::slam::io::PaintSubmapSlices(submap_slices, resolution);
      
      auto msg_ptr = CreateOccupancyGridMsg(painted_slices, resolution, 
                                          map_frame_id_, this->now());
      return *msg_ptr;
    } catch (const std::exception& e) {
      RCLCPP_ERROR(this->get_logger(), "Error loading map: %s", e.what());
      throw; // 重新抛出异常
    }
  }

  std::string pbstream_filename_;
  std::string map_topic_;
  std::string map_frame_id_;
  double resolution_;
  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr publisher_;
};

}  // namespace slam_ros

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  auto node = std::make_shared<slam_ros::MapPublisher>(options);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}